% !Mode:: "TeX:UTF-8" 

\BiChapter{基于连续信念空间的构型规划方法}{Configuration planning under belief space for cooperative localization}

\BiSection{引言}{Introduction}

多智能体系统的构型规划问题的目的是，在系统的运动空间中为网络内的每一个节点搜索一组最优的<控制，状态>序列，使用户自定义的目标函数在规划期限内达到最优值(奖励最大化/损失最小化)，从而完成预设任务。当多智能体系统的状态精确已知时，多智能体的构型规划是一类确定性的马尔可夫决策过程，可以在配置空间中进行有效求解，即只需要考虑状态对目标函数变化的影响。然而，拒止环境存在多种不确定性，任何基于数据融合的状态估计方法都只能获取对节点真实状态的近似估计，无法获得准确的真实状态。
节点的状态不确定性会对配置空间中的构型规划产生影响。其一，节点状态的不确定性会导致基于配置空间的规划方案因为不确定性的扰动而在执行时发生偏离，使得预设任务无法按照预期达到最优。其二，节点状态的不确定性会对规划期限内协同定位过程中相对测量的观测构型产生干扰，从而进一步降低协同定位精度，增大状态估计的不确定性。

与配置空间相比，信念空间将节点在规划期限内的状态分布作为规划变量。由于节点的分布是连续的，因此信念空间本质上具有无穷维度。信念空间规划需要在无穷维度的空间中搜索可行的<控制，状态分布>序列，以使得给定的目标函数最优。因此基于信念空间的构型规划方法可以有效地抑制环境不确定性对规划过程的影响，提高规划算法的鲁棒性。然而，信念空间规划是一类典型的POMDP问题，具有超高的问题复杂度。一般为了简化问题建模，信念空间规划使用参数化的状态信念表征。例如采用高斯分布表征节点信念后，信念空间的规划变量将包括节点在规划时刻的状态预测均值以及状态预测信息矩阵(或协方差矩阵)。

现有的信念空间构型规划方法\citeup{regev2018decentralized,regev2016multi}存在如下问题:

(1) 传统基于RTT，PRM等将状态空间离散化的构型规划方法需要时刻为系统中的每一个节点指定期望位置。对于无法实时分发明确期望状态的多智能体任务，或不存在明确期望状态的任务，这些方法将不适用。

(2) 离散化的构型规划方法在每一个规划时刻都会对每一个节点生成若干条候选路径，导致规划问题时的计算复杂度与规划期限长短和多智能体系统的规模成指数增长。


针对上述问题，本章在连续的信念空间下对构型规划问题进行建模和求解。本章将一类基于数值梯度的连续空间规划方法\citeup{indelman2015planning}拓展到协同定位构型规划问题中来，设计了协同定位构型在连续信念空间的规划框架，避免了RTT，PRM等离散化规划方法指数增长的计算复杂度的问题，该规划算法也不需要为每一个节点都实时分布期望位置。本章将采用最大似然观测假设来预测规划期限内多智能体系统节点之间的相对观测数据。然后调用标准的估计引擎，预测未来时刻的系统状态。最后通过主动协同定位的构型规划数值仿真实验，验证了本文所提方法的可行性和有效性。


\BiSection{问题描述}{Problem formulation}
本节对信念空间的概念进行阐述，将协同定位过程在信念空间进行展开，并给出信念空间规划问题的数学形式。

\BiSubsection{多智能体系统信念空间建模}{Cooperative localization in belief space}

\begin{table}[tbp]
	\bicaption[Tab. NotationsTab]{}{符号表达式的定义总览}{Table$\!$}{Collection of notations and definitions}
	\vspace{0.5em}\centering \wuhao
	\begin{tabular}{cc}
		\toprule[1.5pt]
		符号 & 定义 \\
		\midrule[1pt]
		$\vect{p}^k_i,\vect{W}_j$  & 第$i$个智能体在$k$时刻的状态，以及第$j$个锚节状态 \\
		$\vect{z}^{k}_{i,j}:= \{ \vect{\vec{z}}_{ij}^k, s_{ij}^k \}$  & 第$i$个节点和第$j$个节点在$k$时刻的测量 \\
		$\vect{u}^{k}_{i}$  & 第$i$个节点在$k$时刻的控制输入 \\
		$\vect{X}^k:=\{ \vect{p}^k_1,\cdots,\vect{p}^k_N \}$  & 所有智能体节点在$k$时刻状态的集合 \\
		$\vect{X}^{k_1:k_2}:=\{ \vect{X}^{k_1},\cdots,\vect{X}^{k_2} \}$  & 所有智能体节点在$k_1$和$k_2$时刻状态的集合 \\
		$\vect{Z}^{k}_{i}:=\{ \vect{z}_{i,j}^k | \forall v_j \in \vect{N}_i^k \} = \{ \vect{\vec{Z}}^k_i, \vect{N}_i^k \}$  & 第$i$个智能体节点在第$k$时刻所有测量的集合 \\
		$\mathcal{E}^{k}:= \vect{N}_1^k \bigcup \vect{N}_2^k \bigcup \cdots \bigcup \vect{N}_N^k $ & 智能体集群在第$k$时刻的通信测量拓扑集合 \\	
		$\vect{Z}^k,\vect{Z}^{k_1:k_2},\vect{U}^{k},\vect{U}^{k_1:k_2},\mathcal{E}^{k_1:k_2}$ & 与$\vect{X}^k$ 和 $\vect{X}^{k_1:k_2}$的定义相同\\		
		$\vect{W}:=\{ \vect{W}_1,\cdots,\vect{W}_M \}$ & 环境中所有锚节点的状态\\
		$[\vect{XW}]^k:=\{ \vect{X}^k, \vect{W} \}$ & 第$k$时刻环境中所有节点的状态 \\ 
		$\vect{X}_j^k \in \vect{[XW]}^k$	&  环境中第$j$个节点的状态(智能体或锚节点) \\
		$\mathcal{H}^{0:k}:=\{ \vect{Z}^{0:k}, \vect{U}^{0:k-1}, \vect{W} \}$ & 截止到$k$时刻的所有历史数据 \\
		$\mathcal{H}^{0:k+l|k}:=\{ \mathcal{H}^{0:k}, \vect{U}^{k:k+l-1} \}$ & 所有历史数据以及截至到第$l$个规划时刻的控制输入 \\
		$\vect{E}:=\{ \mathcal{E}^{k+1:k+L}_{iter} | \forall iter \in \mathbb{N}^+_{N_{con}} \}$  & $L$ 个规划期限内所有可能的通信观测拓扑集合 \\
		\bottomrule[1.5pt]
	\end{tabular}
\end{table}

本章假设多智能体系统由$N$个智能体和$O$个固定锚节点组成。其中可移动的智能体均搭载了相同的相对测量和通信设备。它们通过执行相互通信和相对观测行为进行组网，共同组成一个多智能体系统，在拒止的环境中执行相关任务。因此，智能体无法获取自身的精确位置，需要根据配备的传感器数据(如里程计、节点之间相对测量等数据)以及通信设备中解码的其他智能体传输数据对自身位置进行估计。锚节点为一类已知自身精确位置信息的节点，它们装备有与智能体相同的测量和通信设备，可以对其有效覆盖区域内的其他节点进行相对测量并持续广播自身位置信息。在进行对问题进行建模之前，首先介绍多智能体系统一般需要用到的图论的相关知识：

{\textbf{图论：}}使用一个时变无向拓扑图$\mathcal{G} = \{\mathcal{V}, \mathcal{E}(k)\}$描述多智能体系统中的相对观测和通信关系。图中的节点包括智能体和锚节点，统一使用$v_i$或数字索引$i$表示，因此$\mathcal{V}:=\{ v_i| \forall i \in \mathbb{R}^+, i \le N+O \}$为所有节点的集合，包括智能体集合$\mathcal{V}_R$，以及锚节点集合$\mathcal{V}_A$。$\mathcal{E}(k)$为两个相邻节点之间存在的相对测量和通信连通的集合，$\mathcal{E}(k)=\{ v_i \times v_j | \forall  s_{i,j}(k)=1, i \neq j \}$,式中$s_{i,j}(k)$为$s_{\rho}(d_{i,j})$的简化表述。当且仅当$s_{i,j}(k)=1$时，两个节点之间互为邻接节点。使用$\vect{N}_i(k)=\{ v_j | \forall s_{i,j}(k) \in \mathcal{E}(k), s_{i,j}(k) = 1\}$为节点$v_i$在$k$时刻可以进行相互测量和通信的邻接节点集合,用$n_i^k$表示$k$时刻第$i$个智能体邻接节点的个数。

用$k$表示当前时刻，$\vect{p}_i(k)$为当前时刻第$i$个智能体的位置状态，$\vect{W}_j$表示第$j$个锚节点的位置状态。则单个节点的状态转移模型为：
\begin{equation} \label{eq. ACOTO_motionModel}
	\vect{p}_i(k+1) = f_i \left[ \vect{p}_i(k),\vect{u}_i(k),\vect{w}_i \right]
\end{equation}
式中$\vect{p}_i(k)$为第$i$个智能体在$k$时刻的位置状态，$\vect{u}_i(k)$表示第$i$个智能体在$k$时刻的控制输入，$\vect{w}_i$为该智能体运动过程受到的外部随机扰动。
假设该随机扰动服从已知的正态分布$\vect{w}_i \sim \mathcal{N}(\vect{0}, \vect{R}_i)$。
该状态转移模型的概率形式可表示为$p(\vect{p}_i(k+1) | \vect{p}_i(k), \vect{u}_i(k) )$。

与传统方法有所不同，本文研究的观测模型是一组包含两个观测变量的元胞，即$\vect{z}_{ij}^k = \{ \vect{\vec{z}}_{ij}^k , s_{ij}^k \}$。其中$s_{ij}^k$是符合二进制分布的观测连通变量，表征$k$时刻第$i$个节点和第$j$个节点之间是否存在观测；仅当$s_{ij}^k=1$时，即两个节点之间存在观测时，$\vect{\vec{z}}_{ij}^k$为表示测量传感器对该观测连通的实际测量数据。记实际观测数据的观测模型为：
\begin{equation} \label{eq. ACOTO_observationModel1}
	\vect{\vec{z}}_{i,j}(k) = h_{i,j} \left[ \vect{p}_i(k),\vect{X}_j(k), \vect{v}_{i,j} \right]
\end{equation}
式中$\vect{v}_{i,j}$为相对测量设备中存在的随机观测误差，假设该误差依然服从一个已知的正态分布$\vect{v}_{i,j} \sim \mathcal{N}(\vect{0}, \vect{Q}_i)$。
观测量$\vect{\vec{z}}_{ij}$的具体数据格式与智能体携带的相对观测设备的种类有关。例如当相对观测设备为声纳、激光测距等设备时，仅测量两个节点之间的相对距离，则$\vect{\vec{z}}_{ij}$仅为标量。当相对观测设备为雷达等即可测量相对距离又可测量相对角度的设备时，$\vect{\vec{z}}_{i,j} \in \mathbb{R}^{2}$为二维向量。后文将混合使用上标$(\cdot)^k$和括号$(\cdot)(k)$两种符号来表示第$k$个时刻。
该模型的概率形式可记为$p( \vect{\vec{z}}_{i,j}(k) | s_{ij}^k = 1, \vect{p}_i(k),\vect{X}_j(k) )$。

观测连通变量的取值与观测和通信设备的具体工作原理有关。例如，针对无线电测距、超声波测距等观测设备，由于它们符合一类典型圆盘模型的观测条件，
则一般情况下，智能体携带的通信和相对测量设备都会受到实际条件约束（例如最大功率约束等）因而存在最大工作半径，假设智能体和锚节点上的测量设备和通信设备的工作半径是相同的，全都以$\rho$表示，则两个节点之间观测连通变量的数学表达式为：
\begin{equation}	\label{eq. ACOTO_regualtionFunction}
	s_{\rho}(\vect{p}_i^k,\vect{X}_j^k) = \left\lbrace
	\begin{array}{lr}
		1, & d_{ij}(k) \le \rho \\
		0, & d_{ij}(k) > \rho		
	\end{array}			
	\right.
\end{equation}
式中$d_{i,j}(k)$为两个节点之间真实的相对距离：
\begin{equation*}
	d_{i,j}(k) = \left\lVert \vect{p}_i(k) - \vect{X}_j(k) \right\rVert_2
\end{equation*}

二元胞观测$\vect{z}_{ij}^k$的总体概率模型可进行如下分解：
\begin{equation} \label{eq. ACOTO_decomMea}
	\begin{aligned}
		& p(\vect{z}_{i,j}^k|\vect{p}_{i}^k,\vect{X}_j^k) = p(\vect{\vec{z}}_{i,j}^k,s_{ij}^k|\vect{p}_{i}^k,\vect{X}_j^k) \\
		&= p(\vect{\vec{z}}_{i,j}^k|\vect{p}_{i}^k,\vect{X}_j^k,s_{ij}^k = 1)  p(s_{ij}^k = 1|\vect{p}_{i}^k,\vect{X}_j^k)
	\end{aligned}
\end{equation}
对于多智能体系统，为了方便后文的公式推导，表~\ref{Tab. NotationsTab}将所有节点的状态以及系统中其他的内部变量合并为向量形式。

节点的信念状态(belief state) $b(\vect{p}_{ij}^k)$ 是节点状态$\vect{p}_{ij}^k$在给定控制输入和观测数据后的后验概率密度分布：
\begin{equation*}
	b(\vect{p}_{ij}^k) = p ( \vect{p}_{ij}^k | \mathcal{H}^{0:k} ) 
\end{equation*}
所有信念状态的分布共同组成了信念空间(belief space)。因为信念空间不仅包含系统状态，更包含状态分布的不确定性，因此信念空间是一类连续空间，具有无穷多个维度。

类似地，多智能体系统联合状态的信念状态可以表示为：
\begin{equation}
	b(\vect{X}^{0:k}) = p(\vect{X}^{0:k}|\vect{Z}^{0:k},\vect{U}^{0:k-1})
\end{equation} 

当给定一组关于智能体状态的先验分布$p(\vect{p}_i^0),\forall i \in \mathcal{V}_R$，则系统信念的时空传播过程可分解为：
\begin{equation} \label{eq. belief expansion at current step}
	\begin{aligned}
		&b(\vect{X}^{0:k}) = p(\vect{X}^{0:k}|\mathcal{H}^{0:k}) \\
		&= \prod_{i \in \mathcal{V}_R} p(\vect{p}_i^0) \prod_{t=1}^{k} \left[ p(\vect{p}_{i}^{t}|\vect{p}_i^{t-1},\vect{u}_i^{t-1}) \prod_{j\in \vect{N}_i^t} p(z_{i,j}^t|\vect{p}_i^t,\vect{X}_j^t) \right]
	\end{aligned}
\end{equation}

由于本文假设系统受到的所有干扰均为高斯白噪声模型，因此信念状态也符合一个高斯正态密度分布：
\begin{equation} 
	\begin{aligned}
		b(\vect{X}^{0:k})  \sim \mathcal{N}(\vect{\bar{X}}^{0:k},\vect{\Sigma}_{\bar{X}^{0:k}}^k)
	\end{aligned}	
\end{equation} 
式中$\vect{\bar{X}}^{0:k} = \left[ \vect{\bar{X}}^{0},\vect{\bar{X}}^{1},\cdots,\vect{\bar{X}}^{k} \right]^T$是对所有节点位置估计的均值向量（其中仅对应于智能体的状态需要估计，锚节点的位置状态是精确已知的），$\vect{\Sigma}_{\bar{X}^{0:k}}^k$表示在$k$时刻估计出的所有状态的协方差信息。

当给定一组未来$L$个时刻的控制输入$\vect{U}^{k:k+L-1}$，在给定的控制输入下，未来$L$时刻的先验信念可表示为：
\begin{equation} \label{eq. ACOTO_priorBeliefGivenControl}
	\begin{aligned}
		b(\vect{X}^{0:k+L|k}) & = b(\vect{X}^{0:k}) \prod_{t = 1}^{L} p(\vect{X}^{k+t}| \vect{X}^{k+t-1}, \vect{U}^{k+t-1}) \\
		& = p(\vect{X}^{0:k+L} | \mathcal{H}^{0:k+L|k})
	\end{aligned}
\end{equation}

\BiSubsection{协同定位构型规划问题}{Belief space planning for cooperative localization}

如图~\ref{fig. ACTCO_estimator}所示，协同定位是一类状态估计问题，即根据已测量得到的数据和系统模型，求解一个符合信念分布的最优均值向量和协方差矩阵。目前，估计问题典型的求解方法有两类，一类是基于马尔可夫性质(Markov Property)的贝叶斯滤波方法，以著名的卡尔曼滤波、拓展卡尔曼滤波、粒子滤波等方法为代表。此类方法的特点是借助马尔可夫性质，边缘化过去时刻的系统状态，从而实现相邻两个时刻间状态的递归估计；另一类方法是基于优化过程的平滑估计方法，以非线性最小二乘、图优化等理论为典型代表。此类方法的特征是侧重于系统轨迹的估计（同时估计系统的历史状态）而不是仅仅估计系统在当前时刻的状态。后文分别将上述两类估计问题的求解方法称为滤波引擎和平滑引擎，它们的在协同定位任务中的推导过程将在附录~\ref{ch5sec. appendix}中给出。
\begin{figure}[ht]
	\centering
	\includegraphics[width = 0.7\textwidth]{figures_ACTCO/estimator}
	\bicaption[fig. ACTCO_estimator]{}{状态估计问题的输入输出}{Fig.$\!$}{The inputs and outputs of estimation problem}\vspace{0em}
\end{figure}

多智能体系统构型规划问题的目的是，在当前时刻$k$，寻找未来$L$个规划周期内系统中所有节点的控制输入$\vect{U}^{k:k+L-1}$，从而优化一个给定的目标函数(奖励函数/损失函数)。该目标函数的定义一般与系统在未来时刻状态的信念分布有关，例如，
\begin{equation} \label{eq. ACTCO_objective_function}
	J_{k}(\vect{U}^{k:k+L-1}) = \mathop{\mathbb{E}}_{\vect{Z}^{k+1:k+L}} \left[ \sum_{l=1}^{L} c^{l} \left( b(\vect{X}^{k+1:k+l}), \vect{U}^{k:k+l-1} \right)  \right]
\end{equation}
式中$\mathbb{E}[\cdot]$为取期望值的操作符。由于在规划时刻$k$，系统无法获取未来规划期限内的观测数据$\vect{Z}^{k+1:k+L}$，因此目标函数的需要考虑未来所有可能的观测数据，即对观测随机变量取期望。$b(\vect{X}^{k+1:k+l})$为系统在未来$L$个规划周期内的状态的信念分布。$c^l(\cdot)$为与未来第$l$个规划时刻中系统状态有关的目标函数计算式。$c^l(\cdot)$的具体形式与实际的任务配置有关，但一般情况下，目标函数由如下三类指标所组成：
\begin{itemize}
	\item[(1)] 与控制输入量有关的能耗指标$J^C_k$。该指标的目的通常是保证系统选取的控制输入满足一定的燃料消耗约束。该指标的计算通常与控制输入量的范数相关。
	\item[(2)] 与高层任务完成度相关的任务指标$J^T_k$。该指标与系统接受的高层任务的形式有关，适用于确保系统完成一定的期望任务，例如驱使系统搜索特定区域或实现给定的轨迹跟踪等。
	\item[(3)] 与系统自定位精度相关的定位指标$J^L_k$。\textbf{该指标是构型规划与协同定位产生连结的重要环节}。即在规划节点未来控制时，除了考虑能耗约束和任务指标约束外，将协同定位过程的自定位精度作为待优化指标的一部分，从而在保证高层任务完成的同时，让多智能体系统在每一时刻都组成优化的构型，从而产生特定观测拓扑以获取合适、高效的相对测量，从而降低系统协同定位过程的自定位误差。
\end{itemize}

因此在确定了合适的目标函数后，系统的运动策略的确定方法为（此处以目标函数为损失函数为例）：
\begin{equation} \label{eq. ACTCO_original_motion_policy}
	\begin{aligned}
		\vect{U}^{k:k+L-1}_{\star} &= \{ \vect{U}^{k}_{\star},\vect{U}^{k+1}_{\star},...,\vect{U}^{k+L-1}_{\star} \} \\
		&= \mathop{\arg\min}_{\vect{U}^{k:k+L-1} } \quad J_{k}(\vect{U}^{k:k+L-1})
	\end{aligned}
\end{equation}
上述POMDP问题涉及到的系统状态是位处于信念空间的信念状态。由于信念空间具有无穷多个维度，因此上述问题具有极高的问题复杂度，至今仍不存在求取全局最优解的有效方法。

本章的主要任务为，在规划时刻$k$，通过求解式~\eqref{eq. ACTCO_objective_function}-\eqref{eq. ACTCO_original_motion_policy}所定义的构型规划任务，得到多智能体系统节点的控制输入，在完成给定任务指标以及符合能耗指标需求的同时，通过优化系统内部的观测构型，减少协同定位过程的自定位误差，从而增加规划方案对环境不确定性的鲁棒性。


\BiSection{基于数值梯度的连续空间规划方法}{Configuration planning based on numerical gradient} 

由于POMDP问题的超高计算复杂度，实际应用很难得到全局最优解，因此常用方法是采用一类与模型预测控制类似的优化求解过程，从而利用有限的计算资源获得局部最优解。该过程如图~\ref{fig. ACTCO_activePlanningFlowChart}所示，包括如下四个步骤：

\begin{figure}[ht]
	\centering
	\includegraphics[width = 0.8\textwidth]{figures_ACTCO/activePlanningFlowChart}
	\bicaption[fig. ACTCO_activePlanningFlowChart]{}{与模型预测控制类似的POMDP问题通用求解过程}{Fig.$\!$}{A generally MPC-based slover for the POMDP problem}\vspace{0em}
\end{figure}

\begin{itemize}
	\item[(1)] 根据已获取的数据生成对当前环境和自身状态的估计（求解估计问题）。
	\item[(2)] 生成有限个待评估的控制输入；
	\item[(3)] 对上一步生成的每一个控制输入，预测在该输入下系统的未来行为，计算式~\eqref{eq. ACTCO_objective_function}所对应的目标函数值；
	\item[(4)] 返回局部最优解。
\end{itemize}

其中，步骤1)的本质是求解系统当前状态的估计问题，根据已获取的观测数据，求解一组满足式~\eqref{eq. belief expansion at current step}中后验概率密度的最优正态分布；
步骤2)根据状态估计，结合顶层任务需求，生成满足主要任务需求的待评估控制输入。生成候选控制的方法主要有两类，分别为：
\begin{itemize}
	\item[(1)] 基于梯度下降的连续空间生成方法；.
	\item[(2)] 基于概率抽样的离散空间生成方法（如RRT，PRM，$A^{\star}$等算法）
\end{itemize}
步骤3)以式~\eqref{eq. ACTCO_objective_function}的计算结果作为评价指标，衡量每一个候选控制输入的优劣。该过程式整个规划问题的核心，通常涉及对系统未来信念状态的预测。
最后，步骤4）中判断是否需要返回局部最优解的典型依据一般有：

(1) 采用离散的控制生成模块情况下，当超过最大循环次数$r_{max}$时，可终止。

(2) 采用梯度下降的控制生成模块的情况下，当相邻两个控制输入之间的目标函数变化小于特定条件时，可终止。

\begin{algorithm}[htbp]
	\SetAlgoLined
	\KwIn{\\ \quad $b(\vect{X}^{k}) \sim \mathcal{N}(\vect{\bar{X}}^{k}, \vect{\Sigma}^{k})$:  多智能体系统在规划时刻$k$的信念分布$b(\vect{X}^{k})$\\ 
		\quad $b(\vect{X}^{0}) \sim \mathcal{N}(\vect{\bar{X}}^{0}, \vect{\Sigma}^{0})$: 多智能体的初始信念 $b(\vect{X}^{0})$  \\
		\quad $\mathcal{H}^{0:k+L|k} = \{ \mathcal{H}^{0:k}, \vect{U}^{k:k+L-1} \}$ : 包含待评估控制输入的历史数据集合  \\
		%	\quad $r_L$: maximum loop times for the GN iteration }
	}
	\KwOut{\\ \quad $J_k$: 待评估控制输入$\vect{U}^{k:k+L-1}$对应的目标函数值 }
	
	\SetKwProg{Fn}{Function}{:}{end}	
	\Fn{ EvaluateObjective( \textbf{Input} ) } {
		依据最大似然假设预测观测数据 $\vect{Z}^{k+1:k+L}_{ML} \leftarrow $ 式~\eqref{eq. ACOTO_MLobservationPrediction}  \;
		从输入数据集$\mathcal{H}^{0:k}$中提取历史观测数据 $\vect{Z}^{0:k}$ \;
		$\vect{Z}^{0:k+L}_{ML} = \vect{Z}^{k+1:k+L}_{ML} \bigcup \vect{Z}^{0:k}$ \;
		\uIf(\tcc*[f]{重复调用 \text{\textbf{Alg~\ref{alg. FilteringEngineEstimation}}} }){Filtering Engine}{
			\For{$l=0:L-1$}{
				$(\vect{\bar{X}}^{k+l+1}, \vect{\Sigma}^{k+l+1}) = FilteringEngineEKF(\vect{\bar{X}}^{k+l},\vect{\Sigma}^{k+l},\vect{U}^{k+l},\vect{Z}^{k+l+1}_{ML}); $ 			 
			}
		}
		\ElseIf(\tcc*[f]{调用 \text{\textbf{Alg~\ref{alg. SmoothingEngineEstimation}}} }){Smoothing Engine}{
			$(\vect{\bar{X}}^{0:k+L},\vect{\Sigma}^{0:k+L}) = SmoothingEngine(\vect{\bar{X}}^{0}, \vect{\Sigma}^{0},\vect{U}^{0:k+L-1},\vect{Z}^{0:k+L}_{ML})$
		}
		根据$\vect{\bar{X}}^{k+1:k+L},\vect{\Sigma}^{k+1:k+L}$的值计算目标函数$J_k \leftarrow$ 式~\eqref{eq. ACTCO_objective_function}  \\
		\KwRet{$J_k$}\;
	}	
	\AlgoBiCaption{给定候选控制时的目标函数计算算法}{Compute objective function given control candidate}
	\label{alg. EvaluateObjectiveFucntion}
\end{algorithm}


本章将以图~\ref{fig. ACTCO_activePlanningFlowChart}所示的求解框架为基础，设计了一类基于数值梯度的协同定位构型规划框架。数值梯度的思想是通过在初始控制输入中加入微小扰动，通过对比具有微小扰动和初始控制之间的目标函数值，利用数值插值模拟目标函数梯度，从而在初始控制的基础上生成新的待评估控制输入。针对未来观测数据未知的问题，本章将采用最大似然观测假设\citeup{platt2010belief}，使用系统先验估计预测未来规划期限内的观测数据。最后根据预测的观测数据和候选控制输入，调用标准的滤波和平滑引擎从而实现系统在未来规划期限内的状态预测。本章将规划问题的求解过程总结为算法~\ref{alg. EvaluateObjectiveFucntion}----\ref{alg. PlanningScheme}.

\BiSubsection{计算目标函数}{Computation of objective function given control}


本节主要介绍算法~\ref{alg. EvaluateObjectiveFucntion}，即在给定一组控制输入情况下，目标函数~\eqref{eq. ACTCO_objective_function}计算过程的具体实现。由式~\eqref{eq. ACTCO_objective_function}可知，目标函数的计算需要对未来时刻所有可能的观测取期望，由于未来观测在规划时刻是未知的，因此本文采用最大似然观测假设，即
\begin{equation*}
	\vect{Z}^{k+1:k+L}_{ML} = \mathop{\arg \max}_{\vect{Z}^{k+1:k+L}} p( \vect{Z}^{k+1:k+L} | b(\vect{X}^{k}),\vect{U}^{k:k+L-1} )
\end{equation*}
未来观测由两部分组成$\vect{Z}^{k+1:k+L}_{ML} = \{ \vect{\vec{Z}}^{k+1:k+L}_{ML}, \mathcal{E}^{k+1:k+L}_{ML}\}$，因此上式包括的计算包括两部分：
\begin{subequations} \label{eq. ACOTO_MLobservationPrediction}
	\begin{equation} \label{eq. ACOTO_MLobservationPrediction1}
			\vect{\vec{Z}}^{k+1:k+L}_{ML} = h(f)|_{\vect{\bar{X}}^{k};\vect{U}^{k:k+L-1};\forall i,j \in \mathcal{V} \vect{w}_i=0,\vect{v}_{ij}=0} 
	\end{equation}
	\begin{equation}\label{eq. ACOTO_MLobservationPrediction2}
			\mathcal{E}^{k+1:k+L}_{ML}  = s_{\rho}(f) |_{\vect{\bar{X}}^{kL};\vect{U}^{k:k+L-1};\forall i,j \in \mathcal{V} \vect{w}_i=0,\vect{v}_{ij}=0}
	\end{equation}
\end{subequations}

上式中$f(\cdot),h(\cdot),s_{\rho}(\cdot)$分别为式~\eqref{eq. ACOTO_motionModel}当运动噪音为零时的状态转移模型、式~\eqref{eq. ACOTO_observationModel1}中观测噪声为零时的实际观测数据模型以式~\eqref{eq. ACOTO_regualtionFunction}中基于先验状态的观测连通模型。如此，则目标函数~\eqref{eq. ACTCO_objective_function}的计算可转变为：
\begin{equation}
	J_{k}(\vect{U}^{k:k+L-1}) = \left[ \sum_{l=1}^{L} c^{l} \left( b(\vect{X}^{k+1:k+l})_{ML}, \vect{U}^{k:k+l-1} \right) \right]
\end{equation}
其中$b(\vect{X}^{k+1:k+l})_{ML} = p(\vect{X}^{k+1:k+l} | \vect{U}^{k:k+l-1}, \vect{Z}^{k+1:k+l})$。此后，该算法的核心是调用小节~\ref{ch5sec. appendix}中的两类估计问题求解引擎，对规划时刻的历史数据$\mathcal{H}^{0:k}$，候选控制$\vect{U}^{k:k+L-1}$以及预测的观测数据$\vect{Z}^{k+1:k+L}_{ML}$等进行数据融合，预测系统在未来规划期限内的状态信念$b(\vect{X}^{k+1:k+l})_{ML}$。滤波引擎和平滑引擎的具体工作过程，可参照小节~\ref{ch5sec. appendix}中算法~\ref{alg. FilteringEngineEstimation}以及算法~\ref{alg. SmoothingEngineEstimation}。


\BiSubsection{数值梯度计算}{Computation of numerical gradient}

本节主要介绍目标函数在给定控制输入轨迹附近的梯度。由于目标函数的具体形式并未给定，且通常情况下的目标函数并非连续可导函数，因此无法通过求导数方式获取梯度的解析解。本节通过在给定的标称轨迹每一个维度上添加微小扰动，然后通过对扰动前后目标函数值进行差分，得到数值梯度。如算法~\ref{alg. ComputeGradient}所示，数值梯度的计算需要输入多智能体系统在$k$时刻和$0$时刻的信念状态$b(\vect{X}^0),b(\vect{X}^k)$、包含待评估的候选控制输入$\vect{U}^{k:k+L-1}$以及一些系统参数$N,d$等。

算法首先计算在给定标称控制输入下，通过调用算法~\ref{alg. EvaluateObjectiveFucntion}计算系统目标函数的初始取值$J_k$。目标函数的梯度$\Delta \vect{J}_k$是与控制输入$\vect{U}^{k:k+L-1}$同维度的向量，即包含$N*L*d$个标量元素。其中该乘积表示每个节点的控制输入维度为$d$，一共包含$N$个节点，以及在未来$L$个规划期限内的控制输入，因此一共有$N*L*d$个维度。然后在\textit{for}循环中，对控制输入向量的每一个维度$0 \ge \forall l \ge N*L*d-1 $上施加一个微小扰动$\delta_0$，然后计算在该扰动存在下目标函数的取值$J_k^{(l)}$。最后取扰动后的目标函数值与初始目标函数值的差分作为目标函数在该维度上的近似梯度数据。由算法可知，对每一个给定的标称该控制输入，都需要重复调用$N*L*d+1$次目标函数计算算法~\ref{alg. EvaluateObjectiveFucntion}。

\begin{algorithm}[htbp]
	\SetAlgoLined
	\KwIn{\\ \quad $b(\vect{X}^{k}) \sim \mathcal{N}(\vect{\bar{X}}^{k}, \vect{\Sigma}^{k})$: 多智能体系统在$k$时刻的信念$b(\vect{X}^{k})$ \\ 
		\quad $b(\vect{X}^{0}) \sim \mathcal{N}(\vect{\bar{X}}^{0}, \vect{\Sigma}^{0})$: 多智能体系统的初始信念 $b(\vect{X}^{0})$  \\
		\quad $\mathcal{H}^{0:k+L|k} = \{ \mathcal{H}^{0:k}, \vect{U}^{k:k+L-1} \}$ : 包含待评估控制输入的历史数据集合  \\
		\quad $N$ 多智能体系统规模, $d$智能体状态维度. }
	\KwOut{\\ \quad $\Delta J_k $: 目标函数的数值梯度 \; }
	
	\SetKwProg{Fn}{Function}{:}{end}	
	\Fn{ ComputeGradient( \textbf{Input} ) } {
		$J_k = EvaluateObjective(b(\vect{X}^{k}),b(\vect{X}^{0}),\mathcal{H}^{0:k+L|k})$ (\small Alg~\ref{alg. EvaluateObjectiveFucntion}) \;
		
		$\Delta \vect{J}_k = zeros(N*L*d,1)$ \;
		\For{$l=0:N*L*d-1$}{	
			$\delta u = zeros(N*L*d,1)$ , $\delta u(l) = \delta_0$ \tcc*{添加微小扰动}
			从历史数据集$\mathcal{H}^{0:k+L|k}$中提取待评估控制输入$\vect{U}^{k:k+L-1}$ \;
			在待评估控制输入里添加扰动：$\vect{U}^{k:k+L-1}_{(l)} = \vect{U}^{k:k+L-1} + \delta u$\;
			创建数据集$\mathcal{H}^{0:k+L|k}_{(l)} = \left( \mathcal{H}^{0:k+L|k} \setminus \vect{U}^{k:k+L-1}  \right) \bigcup \vect{U}^{k:k+L-1}_{(l)} $\;
			$J_k^{(l)} =  EvaluateObjective(b(\vect{X}^{k}),b(\vect{X}^{0}),\mathcal{H}^{0:k+L|k}_l)$ (\small Alg~\ref{alg. EvaluateObjectiveFucntion}) \;
			$\Delta \vect{J}_k(l) = \dfrac{J_k^{(l)}-J_k}{\delta_0}$ \;
		}
		\KwRet{$\Delta \vect{J}_k$}\;
	}	
	\AlgoBiCaption{数值梯度计算算法}{Compute the numerical gradient of the objective function}
	\label{alg. ComputeGradient}
\end{algorithm}


\begin{algorithm}[H]
	\SetAlgoLined
	\KwIn{\\ \quad $b(\vect{X}^{k}) \sim \mathcal{N}(\vect{\bar{X}}^{k}, \vect{\Sigma}^{k})$: 多智能体系统在$k$时刻的信念$b(\vect{X}^{k})$ \\ 
		\quad $b(\vect{X}^{0}) \sim \mathcal{N}(\vect{\bar{X}}^{0}, \vect{\Sigma}^{0})$: 多智能体系统的初始信念$b(\vect{X}^{0})$  \\
		\quad $\mathcal{H}^{0:k}$:截止到$k$时刻的历史数据 \\
		\quad $\vect{U}^{k:k+L-1}_{(0)}$: 初始的候选控制输入; $L$ 是规划期限\\
		\quad $\lambda$: 梯度下降法的步长; \\
		\quad $N$ 多智能体系统规模; $d$: 节点状态维度. \\
		%	\quad $r_L$: maximum loop times for the GN iteration }
	}
	\KwOut{\\ \quad $\vect{U}^{k:k+L-1}_{\star}$: 规划期限内的优化控制输入}
	
	\SetKwProg{Fn}{Function}{:}{end}	
	\Fn{ ConfigurationPlanning( \textbf{Input} ) } {
		$i=0,J_k^{(0)} = 0$ \tcc*{初始化}
		$\mathcal{H}^{0:k+L|k}_{(0)} = \mathcal{H}^{0:k} \bigcup \vect{U}^{k:k+L-1}_{(0)}$ \;
		\While{true}{			
			$\Delta \vect{J}_k = ComputeGradient(b(\vect{X}^{k}),b(\vect{X}^{0}),\mathcal{H}^{0:k+L|k}_{(i)},N,d) ${\small ({Alg~\ref{alg. ComputeGradient}})} \;
			$\vect{U}^{k:k+L-1}_{(i+1)} = \vect{U}^{k:k+L-1}_{(i)} - \lambda \Delta \vect{J}_k$ \;
			$\mathcal{H}^{0:k+L|k}_{(i+1)} = \mathcal{H}^{0:k} \bigcup \vect{U}^{k:k+L-1}_{(i+1)}$\;
			$J_k^{(i+1)} = EvaluateObjective(b(\vect{X}^{k}),b(\vect{X}^{0}),\mathcal{H}^{0:k+L|k}_{(i+1)})$ {\small (Alg~\ref{alg. EvaluateObjectiveFucntion})}\;
			\If{ $|| \Delta J_k || < \epsilon \text{\textbf{ or }} |\dfrac{J_k^{(i+1)}-J_k^{(i)}}{J_k^{(i+1)}}|<\epsilon \text{\textbf{ or }} i \ge i_{max} $}{
				\KwRet{$\vect{U}^{k:k+L-1}_{\star} = \vect{U}^{k:k+L-1}_{(i+1)}$}\;
			}
			$i = i + 1$;
		}		
	}	
	\AlgoBiCaption{基于数值梯度的信念空间构型规划方法}{Cofiguration planning based on numerical graident in belief space}
	\label{alg. PlanningScheme}
\end{algorithm}

\vspace{+1em}

\BiSubsection{构型规划算法与计算复杂度分析}{Configuration planning algorithm}

算法~\ref{alg. PlanningScheme}为图~\ref{fig. ACTCO_activePlanningFlowChart}所示的规划过程框架。算法的输入数据包括系统在初始时刻的信念分布$b(\vect{X}^0)$，系统在规划的$\k$时刻的信念分布$b(\vect{X}^k)$，$0$到$k$时刻系统从所有传感器收集到的历史观测数据$\mathcal{H}^{0:k}$，未来$L$个规划周期内的多智能体所有节点的初始控制输入$\vect{U}^{k:k+L-1}_{0}$，以及一些系统参数，如梯度下降法固定更新步长$\lambda$、多智能体系统规模$N$，单个智能体节点的状态维度$d$等。

算法的主体是一个条件为真的\textit{while}循环。进入循环后，首先调用算法~\ref{alg. ComputeGradient}，计算目标函数在给定控制输入下的数值梯度向量；随后在第$6$行执行梯度下降过程，产生新的待评估控制输入。第$8$行调用算法~\ref{alg. EvaluateObjectiveFucntion}对新生成的控制输入重新评估其对应的目标函数值。循环的退出条件有三个，首先是数值梯度向量的二范数小于预定阈值，或相邻两个待评估控制输入所对应的目标函数值的变化量小于预定阈值，或循环迭代次数大于某个预定值。这三个条件可以确保规划过程不陷入死循环。

由上一节的分析可知，每一次调用数值梯度计算算法~\ref{alg. ComputeGradient}时，需要重复调用$N*L*d+1$次目标函数计算算法~\ref{alg. EvaluateObjectiveFucntion}。如果将一次目标函数计算过程的运行时间作为一个衡量标准，则算法~\ref{alg. PlanningScheme}在最坏情况下的计算复杂度为$O(i_{max}*(N*L*d+2))$。然而传统的离散状态空间路径规划方法，如RRT、PRM等方法，即使假设目标位置已知，仍需要在每一个规划时刻，对每一个智能体生成一定数量（如$i_{max}$个）的控制候选输入。因此，传统方法在面临具有$N$个节点的多智能体，规划期限为$L$时，在最坏情况下需要评估$i_{max}^{N*L}$个候选控制输入，因此需要运行$i_{max}^{N*L}$次目标函数，因此其计算复杂度最坏为$O(i_{max}^{N*L})$。由此可见，本节设计的方法随系统规模和规划周期长度的关系时线性的，而传统方法则是指数增长关系。因此，本章设计的方法具有较低的计算复杂度。




\BiSection{数值仿真与讨论}{Simulation results and discussions}

本节使用一类基于多智能体系统协同定位方法的巡视任务，以及数值仿真结果，验证本章所设计算法~\ref{alg. EvaluateObjectiveFucntion} - 算法~\ref{alg. PlanningScheme}的有效性和正确性。任务场景如图~\ref{fig. ACTO_simulatinConfiguration}所示，在一片拒止场景中存在三种不同的节点，分别为一个领导者，五个跟随者，以及两个锚节点。其中领导者和跟随者可以运动，但是锚节点始终保持静止。所有节点上全部装备有相互通信和观测传感器，且最大观测距离与最大通信距离保持一致。锚节点的位置是精确已知的，并且始终向其通信观测区域内的其他节点广播自身位置和相互观测数据。领导者和跟随者仅仅知道它们的初始位置，因此需要通过协同定位的方法估计它们在运动过程中每一时刻的位置信息。领导者的任务是沿着给定的路径边点序列（图中红色空心点序列）运动到预设终点。跟随者的目标是通过轨迹规划，优化每一时刻多智能体系统的内部构型，从而提升领导者在运动过程中的定位性能。由图~\ref{fig. ACTO_simulatinConfiguration}的配置可知，领导者的预定义轨迹处于所有锚节点的覆盖范围之外。因此与跟随者的相对观测成为校正领导者自定位误差的唯一外部数据来源。

\begin{figure}[ht]
	\centering
	\includegraphics[width = 0.95\textwidth]{figures_ACTCO/EnvironmentConfigurationEmptyWorld}
	\bicaption[fig. ACTO_simulatinConfiguration]{}{自由空间的场景配置}{Fig.$\!$}{Compositions of the free action space for simulation}\vspace{0em}
\end{figure}

在后文的仿真中，领导者在每一时刻的控制输入由下式生成：
$$\vect{u}_L(k) = \frac{\vect{\bar{p}}_{L}^k - \vect{p}_{W_i}}{||\vect{\bar{p}}_{L} - \vect{p}_{W_i}||_2} \operatorname{min}(u_{max},||\vect{\bar{p}}_{L} - \vect{p}_{W_i}||_2)$$
式中$\vect{\bar{p}}_{L}^k$为$k$时刻领导者对自身位置的估计值，$\vect{\bar{p}}_{W_i}$为领导者给定的路径边点序列中第$i$个路径点的位置，$u_{max}$为单个智能体每一时刻所能行使的最大路程。

本节将进行两组仿真验证，分别测试本章设计方法在自由任务空间和存在障碍的任务空间中的表现。

\BiSubsection{自由空间的任务场景}{Free mission space case}

\begin{table}[htbp]
	\bicaption[tab. ParametersFreeSpace]{}{算法~\ref{alg. EvaluateObjectiveFucntion} -- ~\ref{alg. PlanningScheme}参数设置}{Table$\!$}{Parameters in the Algorithms~\ref{alg. EvaluateObjectiveFucntion}--~\ref{alg. PlanningScheme}}
	\vspace{0.5em}
	\centering
	\wuhao		
	\begin{tabular}{ccccccccc}
		\toprule[1.5pt]		N & $\lambda$ & d & $\epsilon$ & $i_{max}$ & L & $\delta_0$ & $\rho$ & $u_{max}$ \\ 
		\midrule[1pt]
		5 & 5        & 2 & $10^{-4}$  & 20        & 4 & 0.5 & 3 & 0.5  \\
		\bottomrule[1.5pt]
	\end{tabular}
\end{table}

本小节将测试本章所提方法在自由任务空间中对上述巡视任务的性能表现。实验组采用本章所设计信念空间构型规划的算法~\ref{alg. EvaluateObjectiveFucntion} - 算法~\ref{alg. PlanningScheme}，它们的参数设置如表~\ref{tab. ParametersFreeSpace}所示。算法~\ref{alg. PlanningScheme}中每一个跟随者节点的初始控制输入均保持与领导者在未来$L$个时刻的控制相同。式~\eqref{eq. ACTCO_objective_function}中每一时刻的目标函数$c^l(\cdot)$定义为:
$$c^l(\cdot) = 0.9 tr(\vect{\Sigma}_{\vect{\bar{p}}_L^{k+l}}) + 0.1 \sum_{i=1}^{N} tr(\vect{\Sigma}_{\vect{\bar{p}}_{F_i}^{k+l}})$$
式中$\vect{\Sigma}_{\vect{\bar{p}}_L^{k+l}}$表示多智能体系统对领导者节点位置状态在第$l$个规划时刻的预测协方差，$\vect{\Sigma}_{\vect{\bar{p}}_{F_i}^{k+l}}$表示对第$i$个跟随者节点的预测协方差。

\begin{figure}[htbp]
	\centering		
	\subfigure{\label{subfig. discretizationArea}}\addtocounter{subfigure}{-2}
	\subfigure[Naive method]{\subfigure[构型保持方法]{\includegraphics[width = 0.48\textwidth]{figures_ACTCO/ResultsKeepFormation.eps}}}	
	\null\hfill
	\subfigure{\label{subfig. discretizationBoundary}}\addtocounter{subfigure}{-2}
	\subfigure[Proposed method]{\subfigure[信念空间构型规划方法]{\includegraphics[width = 0.48\textwidth]{figures_ACTCO/ResultsBeliefSpacePlanning.eps}}} 	
	\bicaption[fig. SimulationResultsFreeSpace]{}{领导者定位误差表现对比}{Fig.$\!$}{Comparison for localization error of the leader}
\end{figure}

在上述巡视任务中，由于跟随者节点在每一时刻都不存在明确的目标构型，因此传统基于RRT，PRM等离散化空间的路径生成方法在本任务中并不适用于跟随者的路径生成。所以为了验证算法的优越性，本小节将算法~\ref{alg. EvaluateObjectiveFucntion} - 算法~\ref{alg. PlanningScheme}与一类最优PDOP构型保持的控制方法进行对比。图~\ref{fig. ACTO_simulatinConfiguration}中的初始观测拓扑是通过调用精细化求解算法(coarse-to-fine)得到的。在该拓扑下，当所有跟随者对领导者进行观测时，领导者所处的位置具有最优PDOP值。因此对比组采用的策略是使多智能体系统保持与领导者相同的控制输入，从而试图保持最优的PDOP构型。

选用领导者自定位估计协方差矩阵的迹作为衡量指标，将对比组和实验组各自打靶30次，领导者自定位误差在30次打靶仿真中随时间增长的分布如图~\ref{fig. SimulationResultsFreeSpace}所示。由两种方法的对比可知，由于两种方法里面的多智能体系统均无法长期获得绝对位置观测，只能在系统中的某个节点进入锚节点覆盖范围后才能获得对静止锚节点的观测数据，因此两种方法中领导者的定位误差均随着时间的增长而增长。

如图~\ref{fig. SimulationResultsFreeSpace} a) 所示，在采用初始PDOP构型保持的对照组里，领导者的定位误差在$50$步之前的增长较为平缓，这是因为初始情况下跟随者的自定位误差也是比较小。而且在大约$20$和$45$步左右，位于最顶端和最底端的跟随者会进入锚节点的通信观测区域。在来自锚节点的观测数据校正下，多智能体系统的自定位误差得到修正。然而在$50$步之后，多智能体系统由于不存在对外部的绝对位置观测，其自定位误差将逐渐累计。当跟随者的自定位误差较大时，其估计的位置状态已经无法维持初始PDOP构型，因此导致领导者节点的观测数据变少，误差积累速度加快，并在没有锚节点修正的$60-110$步中保持增长。

与之相比，图~\ref{fig. SimulationResultsFreeSpace} b)所示的实验组采用本章设计的基于信念空间和数值梯度的构型规划方法。相似的趋势包括，领导者的自定位误差在前$20$步内保持增长；在约$18$步左右，由于多智能体系统中的节点进入了顶部锚节点的观测通信范围，因此领导者的自定位误差得到了校正；在后期$80-120$步中，多领导者节点的自定位误差也将由于没有外部绝对测量而增长。然而，信念空间构型规划方法的优越性也十分显著。首先，在$20-40$步内，领导者的自定位误差没有明显增长；其次，在约$45$步时，多智能体节点进入了底部第二个锚节点的观测通信范围，从而领导者的自定位误差得到了进一步地降低；再次，在所有节点均退出锚节点范围后，由于合理的构型规划，跟随者始终保持与领导者的观测连通，因此相较于对比组，实验组的领导者自定位误差的增长速度较为缓慢；最后，实验组最终的误差累计远小于对比组。

\begin{figure}[htbp]
	\centering
	\includegraphics[width = 0.95\textwidth]{figures_ACTCO/EnvironmentConfigurationCompWorld}
	\bicaption[fig. ACTO_simulatinConfigurationWithOb]{}{存在障碍的任务空间配置}{Fig.$\!$}{Compositions of mission space with obstacles}\vspace{0em}
\end{figure}

\BiSubsection{存在障碍的任务场景}{mission space with obstacles}
如图~\ref{fig. ACTO_simulatinConfigurationWithOb}所示，存在障碍的任务场景配置基本与自由空间场景类似，只有沿着领导者预定义轨迹两侧存在变化的运动障碍，跟随者也因此被分割为上下两组分离的群体。与自由空间的任务相比，在此类存在障碍的场景显然更为困难。因为在此类空间中，跟随者节点受限于障碍的存在，无法在上下两个分割的运动空间中自由规划，因此限制了优化路径的选取范围。另外，跟随者节点还必须考虑减少与障碍的碰撞。为此，本节的仿真验证实验里，式~\eqref{eq. ACTCO_objective_function}中每一时刻的目标函数$c^l(\cdot)$定义为:
$$c^l(\cdot) = \left[ 0.9 tr(\vect{\Sigma}_{\vect{\bar{p}}_L^{k+l}}) + 0.1 \sum_{i=1}^{N} tr(\vect{\Sigma}_{\vect{\bar{p}}_{F_i}^{k+l}}) \right] (1 + C)$$
式中$C$表示在给定输入$\vect{U}^{k:k+L-1}$作用下，所有跟随者节点在未来$L$个规划期限内与障碍物的碰撞次数。因此跟随者节点在规划未来轨迹时将尽量避免与障碍物的碰撞，然后去寻找可以减少领导者和跟随者定位误差的控制输入。

\begin{figure}[ht]
	\centering
	\includegraphics[width = 0.85\textwidth]{figures_ACTCO/PerformanceObstacles}
	\bicaption[fig. PerformanceObstacles]{}{不同最大观测半径下领导者自定位误差分布情况}{Fig.$\!$}{The distribution of leader's localization error for different ranging thresholds}\vspace{0em}
\end{figure}

由于环境中存在障碍物且前一节对照组所使用的构型保持方法不具备避障功能，因此本节将不再与构型保持方法进行对比。本节将评估不同的的最大观测半径$\rho$对主动协同定位构型规划的影响。本节进行三组实验，分别对应$\rho = 3,4,5 m$的情况，其他配置均与前一节仿真相同，每一组配置重复执行20次。领导者节点的定位误差随时间变化的分布情况如图~\ref{fig. PerformanceObstacles}所示。

通过横向对比上图中对应于$\rho = 3$的变化与图~\ref{fig. SimulationResultsFreeSpace} b) 的变化可知，任务空间中存在障碍会降低规划方法的性能表现。另外通过对比上图中三组实验仿真结果，节点的观测半径越大，规划方法的性能表现越好。


\BiSection{小结}{Conclusion}

本章研究了拒止环境中存在状态不确定性的多智能体系统构型规划问题。设计了一类基于数值梯度的连续信念空间梯度下降构型规划框架； 采用了最大似然观测假设预测，近似多智能体系统在规划期限内的未来观测；使用了标准的估计引擎预测节点在未来时刻的状态信念分布；设计了数值差分的目标函数梯度计算算法，用以生成待评估候选控制。通过理论分析和数值仿真，本章设计的方法与传统的构型规划方法相比有如下优势：

\begin{itemize}
	\item[(1)] 适用于任意形式的目标函数。
	\item[(2)] 不需要为系统总的每一个目标实时分配期望位置。
	\item[(3)] 当多智能体系统规模和规划期限长短发生变化时，本章所提方法仅具备多项式复杂度。相比传统离散化空间的指数复杂度而言，本章设计的方法在大尺度、大规模应用中具有优势。
\end{itemize}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


